﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.ComponentModel;
using System.Globalization;
using System.IO.Ports;
using System.Linq.Expressions;
using System.Windows.Forms;
using System.Text.RegularExpressions;

namespace robotGUI
{
    public enum Modes { DISABLED, PRIMITIVE, HORIZONTAL, BATTLOW, FAILSAFE, STANDUP, FALLDOWN, POSHOLD, BALANCING, BRAKING, FLIPOVER };
    public enum channelFunction { None, Throttle, Steering, ModeSwitch, ActiveProfile, DriftPercent, DriftDelay, TurboBoost, ExtOut_Momentary, ExtOut_Toggle };
    public enum pinState { Low, High };
    public enum statusType { Received, Sent, Timeout, Complete, Null };
    public enum calibrationType { EEBLANK = 1, ENCODER = 2, SPEEDO = 4, SENSORS = 8, RC = 16, BALANCE = 32 };
    public enum profile { Profile_1, Profile_2 };
    public enum failsafe { RC = 1, SERIAL = 2 };

    /* robotSettings is an automatic communication class. Accessing and changing properties 
     * causes a sync with the remote unit over the serial connection. Controls may simply
     * databind to a property. Non-databinding access requires subscription to PropertyChanged
     * events to ensure values are kept current.
     * 
     * PropertyChangedEventArgs may be cast to CommandChangedEventArgs to find the command 
     * corresponding to the altered property.
     * 
     * It uses the serialControl's message transmit functions and receives robotMsg packets
     * through NewMessage.
     */ 

    public class robotSettings : INotifyPropertyChanged
    {
        //Transmit and receive buffers
        List<Command> pendingOutgoing;
        List<Command> pendingIncoming;
        
        //Count of pending incoming items. Properties are not guaranteed current until this is zero.
        [Browsable(false)]
        public int IncomingCount { get { return pendingIncoming.Count; } }

        //True if all properties are current and fresh
        [Browsable(false)]
        public bool Current { get { return freshList.All(element => element); } }

        //True if all properties are stale
        [Browsable(false)]
        public bool Invalid { get { return freshList.All(element => element == false); } }

        bool[] freshList;   //Taglist tracks freshness of properties
        
        Timer timeoutTimer;
        Timer sendTimer;
        public event PropertyChangedEventHandler PropertyChanged;
        public serialControl serialControl;        //from the main form
        const int MAX_USER_NAME_LENGTH = 63;

        //-----------------------------------------------------------------------------------------
        //Remote properties - each corresponds with a Command as sent to remote

        string _version = "(Undefined)";
        [DescriptionAttribute("The running firmware's version. (Read-only)"),
        CategoryAttribute("\t\t\t\t\t\t\tIdentity"),
        ReadOnlyAttribute(true)]
        public string VERSION
        {
            get { checkCurrent(Command.VERSION_REQUEST); return _version; }
            set { SetField(ref _version, value, "VERSION", Command.VERSION_REQUEST); }
        }

        string _userName = "";
        [DescriptionAttribute("The unique identification string."),
        CategoryAttribute("\t\t\t\t\t\t\tIdentity")]
        public string USER_NAME
        {
            get { checkCurrent(Command.USER_NAME); return _userName; }
            set
            {
                if (value.Length <= MAX_USER_NAME_LENGTH) SetField(ref _userName, value, "USER_NAME", Command.USER_NAME);
                else SetField(ref _userName, value.Substring(0, MAX_USER_NAME_LENGTH), "USER_NAME", Command.USER_NAME);
            }
        }

        int _deviceId = -1;
        [DescriptionAttribute("The unique identification number."),
        CategoryAttribute("\t\t\t\t\t\t\tIdentity")]
        public int DEVICE_ID
        {
            get { checkCurrent(Command.DEVICE_ID); return _deviceId; }
            set { SetField(ref _deviceId, value, "DEVICE_ID", Command.DEVICE_ID); }
        }

        ReadOnlyAbilities _abilities;
        [DescriptionAttribute("The firmware's compiled-in set of abilities. (Read-only)"),
        CategoryAttribute("\t\t\t\t\t\tData"),
        ReadOnlyAttribute(true)]
        public ReadOnlyAbilities ABILITIES
        {
            get { checkCurrent(Command.ABILITIES_REQUEST); return _abilities; }
            set { SetField(ref _abilities, value, "ABILITIES", Command.ABILITIES_REQUEST); }
        }

        Abilities _abilityEnable;
        [DescriptionAttribute("The firmware's currently selected set of abilities."),
        CategoryAttribute("\t\t\t\t\t\tData")]
        public Abilities ABILITY_ENABLE
        {
            get { checkCurrent(Command.ABILITY_ENABLE); return _abilityEnable; }
            set
            {
                SetField(ref _abilityEnable, value, "ABILITY_ENABLE", Command.ABILITY_ENABLE);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        Options _optionFlags;
        [DescriptionAttribute("The firmware's collection of option flags."),
        CategoryAttribute("\t\t\t\t\t\tData")]
        public Options OPTION_FLAGS
        {
            get { checkCurrent(Command.OPTION_FLAGS); return _optionFlags; }
            set
            {
                SetField(ref _optionFlags, value, "OPTION_FLAGS", Command.OPTION_FLAGS);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        profile _activeProfile;
        [DescriptionAttribute("Current parameter set in use."),
        CategoryAttribute("\t\t\t\t\t\tData")]
        public profile ACTIVE_PROFILE
        {
            get { checkCurrent(Command.ACTIVE_PROFILE); return _activeProfile; }
            set { SetField(ref _activeProfile, value, "ACTIVE_PROFILE", Command.ACTIVE_PROFILE); }
        }

        Modes _drivingMode;
        [DescriptionAttribute("Currently selected driving mode."),
        CategoryAttribute("\t\t\t\t\t\tData")]
        public Modes DRIVING_MODE
        {
            get { checkCurrent(Command.DRIVING_MODE); return _drivingMode; }
            set { SetField(ref _drivingMode, value, "DRIVING_MODE", Command.DRIVING_MODE); }
        }

        BitMask _dataRequest;
        [DescriptionAttribute("Selects contents of data stream."),
        CategoryAttribute("\t\t\t\t\t\tData")]
        public BitMask DATA_REQUEST
        {
            get { checkCurrent(Command.DATA_REQUEST); return _dataRequest; }
            set
            {
                SetField(ref _dataRequest, value, "DATA_REQUEST", Command.DATA_REQUEST);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        pinState _externalOut;
        [DescriptionAttribute("External output pin state."),
        CategoryAttribute("\t\t\t\t\t\tData")]
        public pinState EXTERNAL_OUT
        {
            get { checkCurrent(Command.EXTERNAL_OUT); return _externalOut; }
            set { SetField(ref _externalOut, value, "EXTERNAL_OUT", Command.EXTERNAL_OUT); }
        }

        Calibration _calbrationDone;
        [DescriptionAttribute("A set of switches indicating which calibrations have been performed and if EEPROM holds valid data."),
        CategoryAttribute("\t\t\t\t\t\tData")]
        public Calibration CALIBRATION_DONE
        {
            get { checkCurrent(Command.CALIBRATION_DONE); return _calbrationDone; }
            set
            {
                SetField(ref _calbrationDone, value, "CALIBRATION_DONE", Command.CALIBRATION_DONE);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        int _eepromSize;
        [DescriptionAttribute("The total size in bytes of the currently used portion of the EEPROM. (Read-only)"),
        CategoryAttribute("\t\t\t\t\t\tData"),
        ReadOnlyAttribute(true)]
        public int EEPROM_SIZE
        {
            get { checkCurrent(Command.EEPROM_SIZE); return _eepromSize; }
            set { SetField(ref _eepromSize, value, "EEPROM_SIZE", Command.EEPROM_SIZE); }
        }

        channelFunction _channel1Function = channelFunction.None;
        [DescriptionAttribute("Function corresponding to RC channel 1 input."),
        CategoryAttribute("\t\t\t\t\tRC Control"),
        DefaultValueAttribute(channelFunction.Steering)]
        public channelFunction CHANNEL1_FUNCTION
        {
            get { checkCurrent(Command.CHANNEL1_FUNCTION); return _channel1Function; }
            set { SetField(ref _channel1Function, value, "CHANNEL1_FUNCTION", Command.CHANNEL1_FUNCTION); }
        }

        channelFunction _channel2Function = channelFunction.None;
        [DescriptionAttribute("Function corresponding to RC channel 2 input."),
        CategoryAttribute("\t\t\t\t\tRC Control"),
        DefaultValueAttribute(channelFunction.Throttle)]
        public channelFunction CHANNEL2_FUNCTION
        {
            get { checkCurrent(Command.CHANNEL2_FUNCTION); return _channel2Function; }
            set { SetField(ref _channel2Function, value, "CHANNEL2_FUNCTION", Command.CHANNEL2_FUNCTION); }
        }

        channelFunction _channel3Function = channelFunction.None;
        [DescriptionAttribute("Function corresponding to RC channel 3 input."),
        CategoryAttribute("\t\t\t\t\tRC Control"),
        DefaultValueAttribute(channelFunction.ModeSwitch)]
        public channelFunction CHANNEL3_FUNCTION
        {
            get { checkCurrent(Command.CHANNEL3_FUNCTION); return _channel3Function; }
            set { SetField(ref _channel3Function, value, "CHANNEL3_FUNCTION", Command.CHANNEL3_FUNCTION); }
        }

        channelFunction _channel4Function = channelFunction.None;
        [DescriptionAttribute("Function corresponding to RC channel 4 input."),
        CategoryAttribute("\t\t\t\t\tRC Control"),
        DefaultValueAttribute(channelFunction.None)]
        public channelFunction CHANNEL4_FUNCTION
        {
            get { checkCurrent(Command.CHANNEL4_FUNCTION); return _channel4Function; }
            set { SetField(ref _channel4Function, value, "CHANNEL4_FUNCTION", Command.CHANNEL4_FUNCTION); }
        }

        int _pulsesPerMeter;
        [DescriptionAttribute("Number of pulses from wheel encoders that corresponds to one meter of forward travel."),
        CategoryAttribute("\t\t\t\tMotor Control"),
        DefaultValueAttribute(0)]
        public int PULSES_PER_METER
        {
            get { checkCurrent(Command.PULSES_PER_METER); return _pulsesPerMeter; }
            set { SetField(ref _pulsesPerMeter, value, "PULSES_PER_METER", Command.PULSES_PER_METER); }
        }

        float _maxSpeedPps;
        [DescriptionAttribute("Maximum wheel speed in pulses per second."),
        CategoryAttribute("\t\t\t\tMotor Control"),
        DefaultValueAttribute(0)]
        public float MAX_SPEED_PPS
        {
            get { checkCurrent(Command.MAX_SPEED_PPS); return _maxSpeedPps; }
            set { SetField(ref _maxSpeedPps, value, "MAX_SPEED_PPS", Command.MAX_SPEED_PPS); }
        }

        int _pwmDeadband;
        [DescriptionAttribute("Offset PWM motor power to overcome wheel inertia."),
        CategoryAttribute("\t\t\t\tMotor Control"),
        DefaultValueAttribute(0)]
        public int PWM_DEADBAND
        {
            get { checkCurrent(Command.PWM_DEADBAND); return _pwmDeadband; }
            set { SetField(ref _pwmDeadband, value, "PWM_DEADBAND", Command.PWM_DEADBAND); }
        }

        PID _leftMotor;
        Command[] leftMotorCommand = new Command[] { Command.LEFTMOTORPID_KP, Command.LEFTMOTORPID_KI, Command.LEFTMOTORPID_KD };
        [DescriptionAttribute("Left motor PID controller."),
        CategoryAttribute("\t\t\t\tMotor Control")]
        public PID LEFTMOTOR
        {
            get { checkCurrent(leftMotorCommand); return _leftMotor; }
            set
            {
                SetField(ref _leftMotor, value, "LEFTMOTOR", leftMotorCommand);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        PID _rightMotor;
        Command[] rightMotorCommand = new Command[] { Command.RIGHTMOTORPID_KP, Command.RIGHTMOTORPID_KI, Command.RIGHTMOTORPID_KD };
        [DescriptionAttribute("Right motor PID controller."),
        CategoryAttribute("\t\t\t\tMotor Control")]
        public PID RIGHTMOTOR
        {
            get { checkCurrent(rightMotorCommand); return _rightMotor; }
            set
            {
                SetField(ref _rightMotor, value, "RIGHTMOTOR", rightMotorCommand);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        float _perfectBalance;
        [DescriptionAttribute("Angle of perfect balance whilst standing upright with the center of gravity directly over the back wheels."),
        CategoryAttribute("\t\t\tBalance Control")]
        public float PERFECT_BALANCE
        {
            get { checkCurrent(Command.PERFECT_BALANCE); return _perfectBalance; }
            set { SetField(ref _perfectBalance, value, "PERFECT_BALANCE", Command.PERFECT_BALANCE); }
        }

        float _lostBalance;
        [DescriptionAttribute("Angle past which to stop trying to balance, give up and fall down."),
        CategoryAttribute("\t\t\tBalance Control")]
        public float LOST_BALANCE
        {
            get { checkCurrent(Command.LOST_BALANCE); return _lostBalance; }
            set { SetField(ref _lostBalance, value, "LOST_BALANCE", Command.LOST_BALANCE); }
        }

        BalanceControls _balanceControls;
        [DescriptionAttribute("Percentage of control during balance. Reduce to make balance easier to control."),
        CategoryAttribute("\t\t\tBalance Control")]
        public BalanceControls BALANCE_CONTROLS
        {
            get { checkCurrent(Command.BALANCE_CONTROLS); return _balanceControls; }
            set
            {
                SetField(ref _balanceControls, value, "BALANCE_CONTROLS", Command.BALANCE_CONTROLS);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        float _posholdStrength;
        [DescriptionAttribute("Position Hold proportional feedback strength."),
        CategoryAttribute("\t\t\tBalance Control")]
        public float POSHOLD_STRENGTH
        {
            get { checkCurrent(Command.POSHOLD_STRENGTH); return _posholdStrength; }
            set { SetField(ref _posholdStrength, value, "POSHOLD_STRENGTH", Command.POSHOLD_STRENGTH); }
        }

        float _posholdLimit;
        [DescriptionAttribute("Maximum Position Hold correction speed."),
        CategoryAttribute("\t\t\tBalance Control")]
        public float POSHOLD_LIMIT
        {
            get { checkCurrent(Command.POSHOLD_LIMIT); return _posholdLimit; }
            set { SetField(ref _posholdLimit, value, "POSHOLD_LIMIT", Command.POSHOLD_LIMIT); }
        }

        PID _balancePid;
        Command[] balancePidCommand = new Command[] { Command.BALANCEPID_KP, Command.BALANCEPID_KI, Command.BALANCEPID_KD };
        [DescriptionAttribute("Balance PID controller. Works to maintain upright position while in balance mode."),
        CategoryAttribute("\t\t\tBalance Control")]
        public PID BALANCEPID
        {
            get { checkCurrent(balancePidCommand); return _balancePid; }
            set
            {
                SetField(ref _balancePid, value, "BALANCE_PID", balancePidCommand);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        DriftPercent _driftPercent;
        [DescriptionAttribute("Amount of drift stabilization to apply."),
        CategoryAttribute("\t\tDrift Control"),
        DefaultValueAttribute(0)]
        public DriftPercent DRIFT_PERCENT
        {
            get { checkCurrent(Command.DRIFT_PERCENT); return _driftPercent; }
            set { 
                SetField(ref _driftPercent, value, "DRIFT_PERCENT", Command.DRIFT_PERCENT);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        float _driftDelay;
        [DescriptionAttribute("Delay before drift stabilization begins to react."),
        CategoryAttribute("\t\tDrift Control"),
        DefaultValueAttribute(0)]
        public float DRIFT_DELAY
        {
            get { checkCurrent(Command.DRIFT_DELAY); return _driftDelay; }
            set { SetField(ref _driftDelay, value, "DRIFT_DELAY", Command.DRIFT_DELAY); }
        }

        float _battFactor;
        [DescriptionAttribute("Battery ADC to Voltage multiplication factor."),
        CategoryAttribute("\tBattery"),
        DefaultValueAttribute(0)]
        public float BATT_FACTOR
        {
            get { checkCurrent(Command.BATT_FACTOR); return _battFactor; }
            set { SetField(ref _battFactor, value, "BATT_FACTOR", Command.BATT_FACTOR); }
        }

        BattLoV _battLowVoltage;
        [DescriptionAttribute("Battery Low Voltage alert threshold."),
        CategoryAttribute("\tBattery"),
        DefaultValueAttribute(0)]
        public BattLoV BATT_LOW_VOLTAGE
        {
            get { checkCurrent(Command.BATT_LOW_VOLTAGE); return _battLowVoltage; }
            set
            {
                SetField(ref _battLowVoltage, value, "BATT_LOW_VOLTAGE", Command.BATT_LOW_VOLTAGE);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        LoopTime _loopTime;
        [DescriptionAttribute("Main loop constant rate timer. Used to set the loop time of the main loop in milliseconds."),
        CategoryAttribute("Timing"),
        DefaultValueAttribute(0)]
        public LoopTime LOOP_TIME
        {
            get { checkCurrent(Command.LOOP_TIME); return _loopTime; }
            set
            {
                SetField(ref _loopTime, value, "LOOP_TIME", Command.LOOP_TIME);
                value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            }
        }

        float _updateRate;
        [DescriptionAttribute("Communication update rate timer. Used to set the number of loop iterations between communication updates."),
        CategoryAttribute("Timing"),
        DefaultValueAttribute(0)]
        public float UPDATE_RATE
        {
            get { checkCurrent(Command.UPDATE_RATE); return _updateRate; }
            set { SetField(ref _updateRate, value, "UPDATE_RATE", Command.UPDATE_RATE); }
        }

        //Read-only property - result of battRaw query
        int _battRaw;
        [Browsable(false)]
        public int BATT_RAW { get { return _battRaw; } }


        //------------------------------------------------------------------------------------------

        public robotSettings(serialControl serialControl)
        {
            this.serialControl = serialControl;
            //setup command queue buffers
            pendingIncoming = new List<Command>();
            pendingOutgoing = new List<Command>();
            //new fresh list
            freshList = Enumerable.Repeat<bool>(false, ((int)robotMsg.LastCommand) + 1).ToArray();
            //Set default values
            _pulsesPerMeter = 0;
            _maxSpeedPps = 0;
            _driftDelay = 0;
            //Instantiate child classes
            _calbrationDone = new Calibration(0xFF);
            _calbrationDone.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _abilities = new ReadOnlyAbilities(0x00);
            _abilityEnable = new Abilities(0x00);
            _abilityEnable.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _optionFlags = new Options(0x00);
            _optionFlags.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _dataRequest = new BitMask(0, Command.DATA_REQUEST, "DATA_REQUEST");
            _dataRequest.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _leftMotor = new PID(this, Command.LEFTMOTORPID_KP, Command.LEFTMOTORPID_KI, Command.LEFTMOTORPID_KD, Command.LEFTMOTORPID_MINGAIN, Command.LEFTMOTORPID_MAXGAIN, Command.LEFTMOTORPID_MINOUT, Command.LEFTMOTORPID_MAXOUT);
            _leftMotor.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _rightMotor = new PID(this, Command.RIGHTMOTORPID_KP, Command.RIGHTMOTORPID_KI, Command.RIGHTMOTORPID_KD, Command.RIGHTMOTORPID_MINGAIN, Command.RIGHTMOTORPID_MAXGAIN, Command.RIGHTMOTORPID_MINOUT, Command.RIGHTMOTORPID_MAXOUT);
            _rightMotor.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _balancePid = new PID(this, Command.BALANCEPID_KP, Command.BALANCEPID_KI, Command.BALANCEPID_KD, Command.BALANCEPID_MINGAIN, Command.BALANCEPID_MAXGAIN, Command.BALANCEPID_MINOUT, Command.BALANCEPID_MAXOUT);
            _balancePid.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _balanceControls = new BalanceControls(0);
            _balanceControls.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _driftPercent = new DriftPercent(0);
            _driftPercent.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _battLowVoltage = new BattLoV(0);
            _battLowVoltage.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            _loopTime = new LoopTime(0);
            _loopTime.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
            //Instantiate timers
            timeoutTimer = new Timer();
            timeoutTimer.Interval = 2000;
            timeoutTimer.Tick += new EventHandler(timeoutTimer_Tick);
            sendTimer = new Timer();
            sendTimer.Tick += new EventHandler(sendTimer_Tick);
        }

        //Daisy chain property changed events
        void childPropertyChanged(object sender, PropertyChangedEventArgs e)
        {
            CommandChangedEventArgs c = (CommandChangedEventArgs)e;
            OnPropertyChanged(c.PropertyName, c.command);
        }

        //Check the freshness of a queried property
        protected void checkCurrent(Command command)
        {
            if (!freshList[(int)command] && !pendingIncoming.Contains(command))
                readParam(command);
        }

        //Check the freshness of a class containing multiple properties
        protected void checkCurrent(Command[] commands)
        {
            foreach (Command command in commands)
                checkCurrent(command);
        }

        /// <summary>
        /// Request a property be refreshed if not current. Subscribers 
        /// to the OnParameterArrived event will be notified when ready. 
        /// </summary>
        /// <param name="command"></param>
        public void getValue(Command command)
        {
            if ((int)command < freshList.Length && freshList[(int)command])
                OnParameterArrived(command);
            else
                readParam(command);
        }

        /// <summary>
        /// Request a list of properties be refreshed if not current. Subscribers 
        /// to the OnParameterArrived event will be notified when ready. 
        /// </summary>
        /// <param name="command"></param>
        public void getValue(Command[] commands)
        {
            foreach (Command command in commands)
                getValue(command);
        }

        //Parameter request buffer. Starts a read or buffers if busy.
        private void readParam(Command command)
        {
            //if Command is not in queue
            if (!pendingIncoming.Contains(command))
            {
                //If queue is not empty
                if (pendingIncoming.Count == 0)
                {
                    //send command
                    serialControl.readParam(command);
                    timeoutTimer.Start();
                }
                //add command to pending queue
                pendingIncoming.Add(command);
            }
        }

        //Timeout
        void timeoutTimer_Tick(object sender, EventArgs e)
        {
            if (pendingIncoming.Count == 0) timeoutTimer.Stop(); //Turn off an idle timer
            else
            {
                //invalidate the value
                if ((int)pendingIncoming[0] < freshList.Length) freshList[(int)pendingIncoming[0]] = false;
                serialControl.readParam(pendingIncoming[0]);  //Retry...
                Status("Timeout waiting for " + pendingIncoming[0].ToString(), pendingIncoming[0], statusType.Timeout);
            }
        }

/*        //InvalidateEvent - called when class requires redisplay
        public delegate void InvalidateEventHandler(object sender, EventArgs e);
        public event InvalidateEventHandler InvalidateEvent;
        public void DoInvalidate()
        {
            if (InvalidateEvent != null)
                InvalidateEvent(this, EventArgs.Empty);
        }
        */
        /// <summary>
        /// Invalidate the contents of robotSettings and force 
        /// all properties to be refreshed from the remote.
        /// </summary>
        public void Invalidate()
        {
            for (int count = 0; count < freshList.Length; count++)
                freshList[count] = false;
            //DoInvalidate();
        }

        //PropertyChanged sends new fresh properties to the remote.
        private void OnPropertyChanged(String info, Command command)
        {
            if (PropertyChanged != null) PropertyChanged(this, new CommandChangedEventArgs(info, command));
            if (command != Command.NONE && freshList[(int)command]) sendValue(command);
        }

        //http://stackoverflow.com/questions/1315621/implementing-inotifypropertychanged-does-a-better-way-exist
        protected bool SetField<T>(ref T field, T value, string propertyName, Command command)
        {
            if (EqualityComparer<T>.Default.Equals(field, value)) return false;
            field = value;
            OnPropertyChanged(propertyName, command);
            return true;
        }

        //SetField for multiple properties
        protected bool SetField<T>(ref T field, T value, string propertyName, Command[] commands)
        {
            if (EqualityComparer<T>.Default.Equals(field, value)) return false;
            field = value;
            foreach (Command command in commands)
                OnPropertyChanged(propertyName, command);
            return true;
        }

        public class ParameterEventArgs : EventArgs
        {
            public Command command;
            public ParameterEventArgs(Command command)
            {
                this.command = command;
            }
        }

        //ParameterArrived is used to notify a getValue() caller the value is now current.
        public delegate void ParameterArrivedEventHandler(object sender, ParameterEventArgs e);
        public event ParameterArrivedEventHandler ParameterArrived;
        public void OnParameterArrived(Command command)
        {
            if (ParameterArrived != null)
            {
                ParameterArrived(this, new ParameterEventArgs(command));
            }
        }

        public class StatusEventArgs : EventArgs
        {
            public string Message;      //friendly message
            public Command Command;     //associated command
            public statusType Type;     //type of status event
            public StatusEventArgs(string Message, Command Command, statusType Type)
            {
                this.Message = Message;
                this.Command = Command;
                this.Type = Type;
            }
        }

        //StatusEvent provides info about what's going on, sends, receives, timeouts and completed.
        public delegate void StatusEventHandler(object sender, StatusEventArgs e);
        public event StatusEventHandler StatusEvent;
        public void Status(string Message, Command command, statusType Type)
        {
            if (StatusEvent != null)
            {
                StatusEvent.Invoke(this, new StatusEventArgs(Message, command, Type));
            }
        }


        /// <summary>
        /// Incoming new message receiver. Needs to be hooked
        /// to a robotMsg provider. Watches for incoming parameters.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        public void newMessage(object sender, NewMessageEventArgs e)
        {
            robotMsg msg = e.msg;
            if (msg.msgType == robotMsg.Type.PAR)
            {
                Status("Got parameter " + ((Command)msg.parCommand).ToString(), (Command)msg.parCommand, statusType.Received);
                switch (msg.parCommand)
                {
                    case (int)Command.ABILITY_ENABLE:
                        ABILITY_ENABLE.Set((int)msg.parData);
                        break;
                    case (int)Command.ACTIVE_PROFILE:
                        ACTIVE_PROFILE = (profile)msg.parData;
                        break;
                    case (int)Command.OPTION_FLAGS:
                        OPTION_FLAGS.Set((int)msg.parData);
                        break;
                    case (int)Command.CALIBRATION_DONE:
                        CALIBRATION_DONE = new Calibration((int)msg.parData);
                        break;
                    case (int)Command.CHANNEL1_FUNCTION:
                        CHANNEL1_FUNCTION = (channelFunction)msg.parData;
                        break;
                    case (int)Command.CHANNEL2_FUNCTION:
                        CHANNEL2_FUNCTION = (channelFunction)msg.parData;
                        break;
                    case (int)Command.CHANNEL3_FUNCTION:
                        CHANNEL3_FUNCTION = (channelFunction)msg.parData;
                        break;
                    case (int)Command.CHANNEL4_FUNCTION:
                        CHANNEL4_FUNCTION = (channelFunction)msg.parData;
                        break;
                    case (int)Command.PULSES_PER_METER:
                        PULSES_PER_METER = (int)msg.parData;
                        break;
                    case (int)Command.MAX_SPEED_PPS:
                        MAX_SPEED_PPS = msg.parData;
                        break;
                    case (int)Command.PWM_DEADBAND:
                        PWM_DEADBAND = (int)msg.parData;
                        break;
                    case (int)Command.LEFTMOTORPID_KP:
                        LEFTMOTOR.P = msg.parData;
                        break;
                    case (int)Command.LEFTMOTORPID_KI:
                        LEFTMOTOR.I = msg.parData;
                        break;
                    case (int)Command.LEFTMOTORPID_KD:
                        LEFTMOTOR.D = msg.parData;
                        break;
                    case (int)Command.LEFTMOTORPID_MINGAIN:
                        LEFTMOTOR.GainLimits.Lower = msg.parData;
                        break;
                    case (int)Command.LEFTMOTORPID_MAXGAIN:
                        LEFTMOTOR.GainLimits.Upper = msg.parData;
                        break;
                    case (int)Command.LEFTMOTORPID_MINOUT:
                        LEFTMOTOR.OutputLimits.Lower = msg.parData;
                        break;
                    case (int)Command.LEFTMOTORPID_MAXOUT:
                        LEFTMOTOR.OutputLimits.Upper = msg.parData;
                        break;
                    case (int)Command.RIGHTMOTORPID_KP:
                        RIGHTMOTOR.P = msg.parData;
                        break;
                    case (int)Command.RIGHTMOTORPID_KI:
                        RIGHTMOTOR.I = msg.parData;
                        break;
                    case (int)Command.RIGHTMOTORPID_KD:
                        RIGHTMOTOR.D = msg.parData;
                        break;
                    case (int)Command.RIGHTMOTORPID_MINGAIN:
                        RIGHTMOTOR.GainLimits.Lower = msg.parData;
                        break;
                    case (int)Command.RIGHTMOTORPID_MAXGAIN:
                        RIGHTMOTOR.GainLimits.Upper = msg.parData;
                        break;
                    case (int)Command.RIGHTMOTORPID_MINOUT:
                        RIGHTMOTOR.OutputLimits.Lower = msg.parData;
                        break;
                    case (int)Command.RIGHTMOTORPID_MAXOUT:
                        RIGHTMOTOR.OutputLimits.Upper = msg.parData;
                        break;
                    case (int)Command.PERFECT_BALANCE:
                        PERFECT_BALANCE = msg.parData;
                        break;
                    case (int)Command.LOST_BALANCE:
                        LOST_BALANCE = msg.parData;
                        break;
                    case (int)Command.BALANCE_CONTROLS:
                        BALANCE_CONTROLS.Value = (int)msg.parData;
                        break;
                    case (int)Command.BALANCEPID_KP:
                        BALANCEPID.P = msg.parData;
                        break;
                    case (int)Command.BALANCEPID_KI:
                        BALANCEPID.I = msg.parData;
                        break;
                    case (int)Command.BALANCEPID_KD:
                        BALANCEPID.D = msg.parData;
                        break;
                    case (int)Command.BALANCEPID_MINGAIN:
                        BALANCEPID.GainLimits.Lower = msg.parData;
                        break;
                    case (int)Command.BALANCEPID_MAXGAIN:
                        BALANCEPID.GainLimits.Upper = msg.parData;
                        break;
                    case (int)Command.BALANCEPID_MINOUT:
                        BALANCEPID.OutputLimits.Lower = msg.parData;
                        break;
                    case (int)Command.BALANCEPID_MAXOUT:
                        BALANCEPID.OutputLimits.Upper = msg.parData;
                        break;
                    case (int)Command.POSHOLD_STRENGTH:
                        POSHOLD_STRENGTH = msg.parData;
                        break;
                    case (int)Command.POSHOLD_LIMIT:
                        POSHOLD_LIMIT = msg.parData;
                        break;
                    case (int)Command.DRIFT_PERCENT:
                        DRIFT_PERCENT.Value = (int)msg.parData;
                        break;
                    case (int)Command.DRIFT_DELAY:
                        DRIFT_DELAY = msg.parData;
                        break;
                    case (int)Command.BATT_FACTOR:
                        BATT_FACTOR = msg.parData;
                        break;
                    case (int)Command.BATT_LOW_VOLTAGE:
                        BATT_LOW_VOLTAGE.Value = msg.parData;
                        break;
                    case (int)Command.LOOP_TIME:
                        LOOP_TIME.Value = msg.parData;
                        break;
                    case (int)Command.UPDATE_RATE:
                        UPDATE_RATE = msg.parData;
                        break;
                    case (int)Command.USER_NAME:
                        USER_NAME = msg.parString;
                        break;
                    case (int)Command.DEVICE_ID:
                        DEVICE_ID = (int)msg.parData;
                        break;
                    case (int)Command.DRIVING_MODE:
                        DRIVING_MODE = (Modes)msg.parData;
                        break;
                    case (int)Command.EXTERNAL_OUT:
                        EXTERNAL_OUT = (msg.parData == 0 ? pinState.Low : pinState.High);
                        break;
                    //Read-only parameters
                    case (int)Command.EEPROM_SIZE:
                        EEPROM_SIZE = (int)msg.parData;
                        break;
                    case (int)Command.VERSION_REQUEST:
                        VERSION = msg.parString;
                        break;
                    case (int)Command.ABILITIES_REQUEST:
                        ABILITIES.Set((int)msg.parData);
                        break;
                    case (int)Command.DATA_REQUEST:
                        DATA_REQUEST.Set((byte)msg.parData);
                        break;
                    //"invisible" parameters
                    case (int)Command.BATT_RAW:
                        _battRaw = (int)msg.parData;
                        break;
                    //Ignore unknown messages
                    default:
                        return;
                }
                //This property is now fresh
                if ((int)msg.parCommand < freshList.Length) freshList[(int)msg.parCommand] = true;
                //Expected this message?
                if (pendingIncoming.Count > 0 && msg.parCommand == (int)pendingIncoming[0])
                {
                    timeoutTimer.Stop();            //stop timeout
                    pendingIncoming.RemoveAt(0);    //remove from pending queue
                    if (pendingIncoming.Count > 0)  //more in queue?
                    {
                        timeoutTimer.Start();       //restart timeout
                        serialControl.readParam(pendingIncoming[0]); //Send the command
                    }
                    else Status("Read Complete.", Command.NONE, statusType.Complete);
                }
                OnParameterArrived((Command)msg.parCommand);
            }
        }

        //Resends command.
        public void Resend(Command command)
        {
            freshList[(int)command] = true;
            sendValue(command);
        }
        
        //Command transmit buffer. Sends command or buffers if busy.
        private void sendValue(Command command)
        {
            //Don't send a stale value or if command is already queued
            if (freshList[(int)command] && !pendingOutgoing.Contains(command))
            {
                //empty queue? send now.
                if (pendingOutgoing.Count == 0)
                {
                    sendRaw(command);
                    sendTimer.Start();
                }
                //Add to pending queue
                pendingOutgoing.Add(command);
            }   
        }

        //Outgoing commands are timed. The tick sends the 
        // next command from the buffer or stops if empty.
        void sendTimer_Tick(object sender, EventArgs e)
        {
            //stop timer
            sendTimer.Stop();
            //pop command off front of queue
            if (pendingOutgoing.Count > 0)
                pendingOutgoing.RemoveAt(0);
            if (pendingOutgoing.Count > 0)
            {
                //Send next command in line
                sendRaw(pendingOutgoing[0]);
                sendTimer.Start();
            }
        }

        //Low-level command => value transmission.
        void sendRaw(Command command)
        {
            freshList[(int)command] = true;
            Status("Sent parameter " + command.ToString(), command, statusType.Sent);
            switch (command)
            {
                case Command.ABILITY_ENABLE:
                    serialControl.sendValue(command, ABILITY_ENABLE.Get());
                    break;
                case Command.ACTIVE_PROFILE:
                    serialControl.sendValue(command, (int)ACTIVE_PROFILE);
                    Invalidate();
                    break;
                case Command.OPTION_FLAGS:
                    serialControl.sendValue(command, OPTION_FLAGS.Get());
                    break;
                case Command.CALIBRATION_DONE:
                    serialControl.sendValue(command, CALIBRATION_DONE.Get());
                    break;
                case Command.CHANNEL1_FUNCTION:
                    serialControl.sendValue(command, (int)CHANNEL1_FUNCTION);
                    break;
                case Command.CHANNEL2_FUNCTION:
                    serialControl.sendValue(command, (int)CHANNEL2_FUNCTION);
                    break;
                case Command.CHANNEL3_FUNCTION:
                    serialControl.sendValue(command, (int)CHANNEL3_FUNCTION);
                    break;
                case Command.CHANNEL4_FUNCTION:
                    serialControl.sendValue(command, (int)CHANNEL4_FUNCTION);
                    break;
                case Command.PULSES_PER_METER:
                    serialControl.sendValue(command, PULSES_PER_METER);
                    break;
                case Command.MAX_SPEED_PPS:
                    serialControl.sendValue(command, MAX_SPEED_PPS);
                    break;
                case Command.PWM_DEADBAND:
                    serialControl.sendValue(command, PWM_DEADBAND);
                    break;
                case Command.LEFTMOTORPID_KP:
                    serialControl.sendValue(command, LEFTMOTOR.P);
                    break;
                case Command.LEFTMOTORPID_KI:
                    serialControl.sendValue(command, LEFTMOTOR.I);
                    break;
                case Command.LEFTMOTORPID_KD:
                    serialControl.sendValue(command, LEFTMOTOR.D);
                    break;
                case Command.LEFTMOTORPID_MINGAIN:
                    serialControl.sendValue(command, LEFTMOTOR.GainLimits.Lower);
                    break;
                case Command.LEFTMOTORPID_MAXGAIN:
                    serialControl.sendValue(command, LEFTMOTOR.GainLimits.Upper);
                    break;
                case Command.LEFTMOTORPID_MINOUT:
                    serialControl.sendValue(command, LEFTMOTOR.OutputLimits.Lower);
                    break;
                case Command.LEFTMOTORPID_MAXOUT:
                    serialControl.sendValue(command, LEFTMOTOR.OutputLimits.Upper);
                    break;
                case Command.RIGHTMOTORPID_KP:
                    serialControl.sendValue(command, RIGHTMOTOR.P);
                    break;
                case Command.RIGHTMOTORPID_KI:
                    serialControl.sendValue(command, RIGHTMOTOR.I);
                    break;
                case Command.RIGHTMOTORPID_KD:
                    serialControl.sendValue(command, RIGHTMOTOR.D);
                    break;
                case Command.RIGHTMOTORPID_MINGAIN:
                    serialControl.sendValue(command, RIGHTMOTOR.GainLimits.Lower);
                    break;
                case Command.RIGHTMOTORPID_MAXGAIN:
                    serialControl.sendValue(command, RIGHTMOTOR.GainLimits.Upper);
                    break;
                case Command.RIGHTMOTORPID_MINOUT:
                    serialControl.sendValue(command, RIGHTMOTOR.OutputLimits.Lower);
                    break;
                case Command.RIGHTMOTORPID_MAXOUT:
                    serialControl.sendValue(command, RIGHTMOTOR.OutputLimits.Upper);
                    break;
                case Command.PERFECT_BALANCE:
                    serialControl.sendValue(command, PERFECT_BALANCE);
                    break;
                case Command.LOST_BALANCE:
                    serialControl.sendValue(command, LOST_BALANCE);
                    break;
                case Command.BALANCE_CONTROLS:
                    serialControl.sendValue(command, BALANCE_CONTROLS.Value);
                    break;
                case Command.BALANCEPID_KP:
                    serialControl.sendValue(command, BALANCEPID.P);
                    break;
                case Command.BALANCEPID_KI:
                    serialControl.sendValue(command, BALANCEPID.I);
                    break;
                case Command.BALANCEPID_KD:
                    serialControl.sendValue(command, BALANCEPID.D);
                    break;
                case Command.BALANCEPID_MINGAIN:
                    serialControl.sendValue(command, BALANCEPID.GainLimits.Lower);
                    break;
                case Command.BALANCEPID_MAXGAIN:
                    serialControl.sendValue(command, BALANCEPID.GainLimits.Upper);
                    break;
                case Command.BALANCEPID_MINOUT:
                    serialControl.sendValue(command, BALANCEPID.OutputLimits.Lower);
                    break;
                case Command.BALANCEPID_MAXOUT:
                    serialControl.sendValue(command, BALANCEPID.OutputLimits.Upper);
                    break;
                case Command.POSHOLD_STRENGTH:
                    serialControl.sendValue(command, POSHOLD_STRENGTH);
                    break;
                case Command.POSHOLD_LIMIT:
                    serialControl.sendValue(command, POSHOLD_LIMIT);
                    break;
                case Command.DRIFT_PERCENT:
                    serialControl.sendValue(command, DRIFT_PERCENT.Value);
                    break;
                case Command.DRIFT_DELAY:
                    serialControl.sendValue(command, DRIFT_DELAY);
                    break;
                case Command.BATT_FACTOR:
                    serialControl.sendValue(command, BATT_FACTOR);
                    break;
                case Command.BATT_LOW_VOLTAGE:
                    serialControl.sendValue(command, BATT_LOW_VOLTAGE.Value);
                    break;
                case Command.LOOP_TIME:
                    serialControl.sendValue(command, LOOP_TIME.Value);
                    break;
                case Command.UPDATE_RATE:
                    serialControl.sendValue(command, UPDATE_RATE);
                    break;
                case Command.USER_NAME:
                    serialControl.sendValue(command, USER_NAME);
                    break;
                case Command.DEVICE_ID:
                    serialControl.sendValue(command, DEVICE_ID);
                    break;
                case Command.DRIVING_MODE:
                    serialControl.sendValue(command, (int)DRIVING_MODE);
                    break;
                case Command.EXTERNAL_OUT:
                    serialControl.sendValue(command, (int)EXTERNAL_OUT);
                    break;
                case Command.DATA_REQUEST:
                    serialControl.sendValue(command, DATA_REQUEST.Get());
                    break;
                default:
                    throw new UnknownCommandException("Invalid parameter message comand: " + command.ToString());
            }
        }

        //-----------------------------------------------------------------------------------------
        //Special data display classes

        //Superclass of PropertChangedEventArgs provides the command itself.
        public class CommandChangedEventArgs : PropertyChangedEventArgs
        {
            public Command command;
            public CommandChangedEventArgs(string propertyName, Command command)
                : base(propertyName)
            {
                this.command = command;
            }
        }

        public class UnitConverter : TypeConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context, Type destinationType)
            {
                if (destinationType == typeof(Unit)) return true;
                return base.CanConvertTo(context, destinationType);
            }
            public override object ConvertTo(ITypeDescriptorContext context, CultureInfo culture, object value, Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is Unit)
                {
                    Unit c = (Unit)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = (string)value;
                        Unit original = (Unit)context.PropertyDescriptor.GetValue(context.Instance);
                        float v = float.Parse(s.Replace(original.unit, ""));
                        Unit u = new Unit(v, original.unit, original.command, original.name);
                        return u;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type Unit");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }



        [TypeConverterAttribute(typeof(UnitConverter))]
        public class Unit : INotifyPropertyChanged
        {
            public string name;
            public Command command;
            public string unit;
            float _value;
            public float Value
            {
                get { return _value; }
                set
                {
                    if (_value != value)
                    {
                        _value = value;
                        OnPropertyChanged(name, command);
                    }
                }
            }
            public Unit(float value, string Unit, Command Command, string Name)
            {
                unit = Unit;
                name = Name;
                command = Command;
                _value = value;
            }
            public event PropertyChangedEventHandler PropertyChanged;
            private void OnPropertyChanged(String info, Command command)
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs(info, command));
                }
            }
            public override string ToString()
            {
                return _value.ToString() + unit;
            }
        }

        //====================================================================
        // Battery low voltage
        [TypeConverterAttribute(typeof(BattLoVConverter))]
        public class BattLoV : INotifyPropertyChanged
        {
            float _value;
            public float Value
            {
                get { return _value; }
                set
                {
                    if (_value != value)
                    {
                        _value = value;
                        OnPropertyChanged();
                    }
                }
            }
            public BattLoV(float value)
            {
                _value = value;
            }
            public event PropertyChangedEventHandler PropertyChanged;
            private void OnPropertyChanged()
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs("BATT_LOW_VOLTAGE", Command.BATT_LOW_VOLTAGE));
                }
            }
            public override string ToString()
            {
                return _value.ToString() + "v";
            }
        }
        public class BattLoVConverter : TypeConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context, Type destinationType)
            {
                if (destinationType == typeof(BattLoV)) return true;
                return base.CanConvertTo(context, destinationType);
            }
            public override object ConvertTo(ITypeDescriptorContext context, CultureInfo culture, object value, Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is BattLoV)
                {
                    BattLoV c = (BattLoV)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = (string)value;
                        float v = float.Parse(Regex.Replace(s, "[^0-9.]", ""));
                        BattLoV u = new BattLoV(v);
                        return u;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type BattLoV");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }


        //====================================================================
        // Loop time
        [TypeConverterAttribute(typeof(LoopTimeConverter))]
        public class LoopTime : INotifyPropertyChanged
        {
            float _value;
            public float Value
            {
                get { return _value; }
                set
                {
                    if (_value != value)
                    {
                        _value = value;
                        OnPropertyChanged();
                    }
                }
            }
            public LoopTime(float value)
            {
                _value = value;
            }
            public event PropertyChangedEventHandler PropertyChanged;
            private void OnPropertyChanged()
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs("LOOP_TIME", Command.LOOP_TIME));
                }
            }
            public override string ToString()
            {
                return _value.ToString() + "ms";
            }
        }
        public class LoopTimeConverter : TypeConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context, Type destinationType)
            {
                if (destinationType == typeof(LoopTime)) return true;
                return base.CanConvertTo(context, destinationType);
            }
            public override object ConvertTo(ITypeDescriptorContext context, CultureInfo culture, object value, Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is LoopTime)
                {
                    LoopTime c = (LoopTime)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = (string)value;
                        float v = float.Parse(Regex.Replace(s, "[^0-9.]", ""));
                        LoopTime u = new LoopTime(v);
                        return u;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type LoopTime");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

        //====================================================================
        // BalanceControls
        [TypeConverterAttribute(typeof(BalanceControlsConverter))]
        public class BalanceControls : INotifyPropertyChanged
        {
            int _value;
            public int Value
            {
                get { return _value; }
                set
                {
                    if (_value != value)
                    {
                        _value = value;
                        OnPropertyChanged();
                    }
                }
            }
            public BalanceControls(int value)
            {
                _value = value;
            }
            public event PropertyChangedEventHandler PropertyChanged;
            private void OnPropertyChanged()
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs("BALANCE_CONTROLS", Command.BALANCE_CONTROLS));
                }
            }
            public override string ToString()
            {
                return _value.ToString() + "%";
            }
        }
        public class BalanceControlsConverter : TypeConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context, Type destinationType)
            {
                if (destinationType == typeof(BalanceControls)) return true;
                return base.CanConvertTo(context, destinationType);
            }
            public override object ConvertTo(ITypeDescriptorContext context, CultureInfo culture, object value, Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is BalanceControls)
                {
                    BalanceControls c = (BalanceControls)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = (string)value;
                        float v = float.Parse(Regex.Replace(s, "[^0-9.]", ""));
                        BalanceControls u = new BalanceControls((int)v);
                        return u;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type BalanceControls");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

        //====================================================================
        // DriftPercent
        [TypeConverterAttribute(typeof(DriftPercentConverter))]
        public class DriftPercent : INotifyPropertyChanged
        {
            int _value;
            public int Value
            {
                get { return _value; }
                set
                {
                    if (_value != value)
                    {
                        _value = value;
                        OnPropertyChanged();
                    }
                }
            }
            public DriftPercent(int value)
            {
                _value = value;
            }
            public event PropertyChangedEventHandler PropertyChanged;
            private void OnPropertyChanged()
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs("DRIFT_PERCENT", Command.DRIFT_PERCENT));
                }
            }
            public override string ToString()
            {
                return _value.ToString() + "%";
            }
        }
        public class DriftPercentConverter : TypeConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context, Type destinationType)
            {
                if (destinationType == typeof(DriftPercent)) return true;
                return base.CanConvertTo(context, destinationType);
            }
            public override object ConvertTo(ITypeDescriptorContext context, CultureInfo culture, object value, Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is DriftPercent)
                {
                    DriftPercent c = (DriftPercent)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = (string)value;
                        float v = float.Parse(Regex.Replace(s, "[^0-9.]", ""));
                        DriftPercent u = new DriftPercent((int)v);
                        return u;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type DriftPercent");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }




        //Calibrations display class
        [TypeConverterAttribute(typeof(CalibrationConverter)),
        DescriptionAttribute("Expand to alter calibrations.")]
        public class Calibration: INotifyPropertyChanged
        {
            bool _calBalance;
            [DescriptionAttribute("Whether the balancing algorithm has been calibrated."),
            DefaultValueAttribute(true)]
            public bool CAL_BALANCE
            {
                get { return _calBalance; }
                set { SetField(ref _calBalance, value, "CAL_BALANCE", Command.CALIBRATION_DONE); }
            }
            bool _calRc;
            [DescriptionAttribute("Whether the RC stick centers and limits have been calibrated."),
            DefaultValueAttribute(true)]
            public bool CAL_RC
            {
                get { return _calRc; }
                set { SetField(ref _calRc, value, "CAL_RC", Command.CALIBRATION_DONE); }
            }
            bool _calSensors;
            [DescriptionAttribute("Whether the accelerometer has been calibrated."),
            DefaultValueAttribute(true)]
            public bool CAL_SENSORS
            {
                get { return _calSensors; }
                set { SetField(ref _calSensors, value, "CAL_SENSORS", Command.CALIBRATION_DONE); }
            }
            bool _calSpeedo;
            [DescriptionAttribute("Whether the top speed has been measured."),
            DefaultValueAttribute(true)]
            public bool CAL_SPEEDO
            {
                get { return _calSpeedo; }
                set { SetField(ref _calSpeedo, value, "CAL_SPEEDO", Command.CALIBRATION_DONE); }
            }
            [DescriptionAttribute("Whether the wheel encoders have been calibrated."),
            DefaultValueAttribute(true)]
            bool _calEncoder;
            public bool CAL_ENCODER
            {
                get { return _calEncoder; }
                set { SetField(ref _calEncoder, value, "CAL_ENCODER", Command.CALIBRATION_DONE); }
            }
            bool _calEeblank;
            [DescriptionAttribute("Whether the EEPROM is uninitialized."),
            DefaultValueAttribute(true)]
            public bool CAL_EEBLANK
            {
                get { return _calEeblank; }
                set { SetField(ref _calEeblank, value, "CAL_EEBLANK", Command.CALIBRATION_DONE); }
            }


            public Calibration()
            {
                CAL_BALANCE = CAL_EEBLANK = CAL_RC = CAL_SENSORS = CAL_SPEEDO = true;
            }

            public void Set(int mask)
            {
                CAL_EEBLANK = ((mask & 1) == 1);
                CAL_ENCODER = ((mask & 2) == 2);
                CAL_SPEEDO = ((mask & 4) == 4);
                CAL_SENSORS = ((mask & 8) == 8);
                CAL_RC = ((mask & 16) == 16);
                CAL_BALANCE = ((mask & 32) == 32);
            }

            public int Get()
            {
                return (CAL_EEBLANK ? 1 : 0) + (CAL_ENCODER ? 2 : 0) + (CAL_SPEEDO ? 4 : 0) + (CAL_SENSORS ? 8 : 0) + (CAL_RC ? 16 : 0) + (CAL_BALANCE ? 32 : 0);
            }

            public Calibration(int mask)
            {
                Set(mask);
            }

            public override string ToString()
            {
                if (Get() == 0) return "NONE";
                return (CAL_EEBLANK ? "BLANK " : "") + (CAL_ENCODER ? "ENCODER " : "") + (CAL_SPEEDO ? "SPEEDO " : "") + (CAL_SENSORS ? "SENSORS " : "") + (CAL_RC ? "RC " : "") + (CAL_BALANCE ? "BALANCE " : "");
            }

            public event PropertyChangedEventHandler PropertyChanged;

            private void OnPropertyChanged(String info, Command command)
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs(info, command));
                }
            }

            protected bool SetField<T>(ref T field, T value, string propertyName, Command command)
            {
                if (EqualityComparer<T>.Default.Equals(field, value)) return false;
                field = value;
                OnPropertyChanged(propertyName, command);
                return true;
            }

        }

        public class CalibrationConverter : ExpandableObjectConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context,
                                          System.Type destinationType)
            {
                if (destinationType == typeof(Calibration)) return true;
                return base.CanConvertTo(context, destinationType);
            }

            public override object ConvertTo(ITypeDescriptorContext context,
                                   CultureInfo culture,
                                   object value,
                                   System.Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is Calibration)
                {
                    Calibration c = (Calibration)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = ((string)value).ToUpper();
                        int val = 0;
                        if (s.Contains("BLANK")) val += 1;
                        if (s.Contains("ENCODER")) val += 2;
                        if (s.Contains("SPEEDO")) val += 4;
                        if (s.Contains("SENSORS")) val += 8;
                        if (s.Contains("RC")) val += 16;
                        if (s.Contains("BALANCE")) val += 32;
                        Calibration c = new Calibration(val);
                        return c;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type Calibration");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

        //Option display class
        [TypeConverterAttribute(typeof(OptionConverter)),
        DescriptionAttribute("Bitfield describing options.")]
        public class Options : INotifyPropertyChanged
        {
            bool _flip_reverse;
            [DescriptionAttribute("Whether to reverse controls in order to maintain vehicle's forward orientation when inverted."),
            DefaultValueAttribute(true)]
            public bool OPTION_FLIP_REVERSE
            {
                get { return _flip_reverse; }
                set { SetField(ref _flip_reverse, value, "OPTION_FLIP_REVERSE", Command.OPTION_FLAGS); }
            }
            bool _auto_balance;
            [DescriptionAttribute("Whether to automatically catch balance if standing upright."),
            DefaultValueAttribute(true)]
            public bool OPTION_AUTO_BALANCE
            {
                get { return _auto_balance; }
                set { SetField(ref _auto_balance, value, "OPTION_AUTO_BALANCE", Command.OPTION_FLAGS); }
            }

            public Options()
            {
                _flip_reverse = _auto_balance = true;
            }

            public void Set(int mask)
            {
                OPTION_FLIP_REVERSE = ((mask & 1) == 1);
                OPTION_AUTO_BALANCE = ((mask & 2) == 2);
            }

            public int Get()
            {
                return (_flip_reverse ? 1 : 0) + (_auto_balance ? 2 : 0);
            }

            public Options(int mask)
            {
                Set(mask);
            }

            public override string ToString()
            {
                if (Get() == 0) return "NONE";
                return (_flip_reverse ? "FLIP_REVERSE " : "") + (_auto_balance ? "AUTO_BALANCE " : "");
            }

            public event PropertyChangedEventHandler PropertyChanged;

            private void OnPropertyChanged(String info, Command command)
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs(info, command));
                }
            }

            protected bool SetField<T>(ref T field, T value, string propertyName, Command command)
            {
                if (EqualityComparer<T>.Default.Equals(field, value)) return false;
                field = value;
                OnPropertyChanged(propertyName, command);
                return true;
            }

        }

        public class OptionConverter : ExpandableObjectConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context,
                                          System.Type destinationType)
            {
                if (destinationType == typeof(Calibration)) return true;
                return base.CanConvertTo(context, destinationType);
            }

            public override object ConvertTo(ITypeDescriptorContext context,
                                   CultureInfo culture,
                                   object value,
                                   System.Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is Options)
                {
                    Options c = (Options)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = ((string)value).ToUpper();
                        int val = 0;
                        if (s.Contains("FLIP_REVERSE")) val += 1;
                        if (s.Contains("AUTO_BALANCE")) val += 2;
                        Options c = new Options(val);
                        return c;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type Options");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

        //Abilities display class
        [TypeConverterAttribute(typeof(AbilityConverter)),
        DescriptionAttribute("Bitfield describing abilities.")]
        public class Abilities : INotifyPropertyChanged
        {
            bool _rc;
            [DescriptionAttribute("Whether the RC Receiver channel inputs are enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_RC
            {
                get { return _rc; }
                set { SetField(ref _rc, value, "ABILITY_RC", Command.ABILITY_ENABLE); }
            }
            bool _acc;
            [DescriptionAttribute("Whether the Accelerometer is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_ACC
            {
                get { return _acc; }
                set { SetField(ref _acc, value, "ABILITY_ACC", Command.ABILITY_ENABLE); }
            }
            bool _gyro;
            [DescriptionAttribute("Whether the gyroscope is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_GYRO
            {
                get { return _gyro; }
                set { SetField(ref _gyro, value, "ABILITY_GYRO", Command.ABILITY_ENABLE); }
            }
            bool _encoders;
            [DescriptionAttribute("Whether the wheel rotation encoders are present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_ENCODERS
            {
                get { return _encoders; }
                set { SetField(ref _encoders, value, "ABILITY_ENCODERS", Command.ABILITY_ENABLE); }
            }
            [DescriptionAttribute("Whether the Drift Correction algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            bool _drift;
            public bool ABILITY_DRIFT
            {
                get { return _drift; }
                set { SetField(ref _drift, value, "ABILTIY_DRIFT", Command.ABILITY_ENABLE); }
            }
            bool _roll;
            [DescriptionAttribute("Whether the Roll Stabilization algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_ROLL
            {
                get { return _roll; }
                set { SetField(ref _roll, value, "ABILITY_ROLL", Command.ABILITY_ENABLE); }
            }
            bool _balance;
            [DescriptionAttribute("Whether the Self Balance algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_BALANCE
            {
                get { return _balance; }
                set { SetField(ref _balance, value, "ABILITY_BALANCE", Command.ABILITY_ENABLE); }
            }
            bool _poshold;
            [DescriptionAttribute("Whether the Position Hold algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_POSHOLD
            {
                get { return _poshold; }
                set { SetField(ref _poshold, value, "ABILITY_POSHOLD", Command.ABILITY_ENABLE); }
            }


            public Abilities()
            {
                _rc = _acc = _gyro = _encoders = _roll = _drift = _balance = _poshold = true;
            }

            public void Set(int mask)
            {
                ABILITY_RC = ((mask & 1) == 1);
                ABILITY_ACC = ((mask & 2) == 2);
                ABILITY_GYRO = ((mask & 4) == 4);
                ABILITY_ENCODERS = ((mask & 8) == 8);
                ABILITY_DRIFT = ((mask & 16) == 16);
                ABILITY_ROLL = ((mask & 32) == 32);
                ABILITY_BALANCE = ((mask & 64) == 64);
                ABILITY_POSHOLD = ((mask & 128) == 128);
            }

            public int Get()
            {
                return (_rc ? 1 : 0) + (_acc ? 2 : 0) + (_gyro ? 4 : 0) + (_encoders ? 8 : 0) + (_drift ? 16 : 0) + (_roll ? 32 : 0) + (_balance ? 64 : 0) + (_poshold ? 128 : 0);
            }

            public Abilities(int mask)
            {
                Set(mask);
            }

            public override string ToString()
            {
                if (Get() == 0) return "NONE";
                if (Get() == 255) return "ALL";
                return (_rc ? "RC " : "") + (_acc ? "ACC " : "") + (_gyro ? "GYRO " : "") + (_encoders ? "ENCODERS " : "") + (_drift ? "DRIFT " : "") + (_roll ? "ROLL " : "") + (_balance ? "BALANCE " : "") + (_poshold ? "POSHOLD" : "");
            }

            public event PropertyChangedEventHandler PropertyChanged;

            private void OnPropertyChanged(String info, Command command)
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs(info, command));
                }
            }

            protected bool SetField<T>(ref T field, T value, string propertyName, Command command)
            {
                if (EqualityComparer<T>.Default.Equals(field, value)) return false;
                field = value;
                OnPropertyChanged(propertyName, command);
                return true;
            }

        }

        //Read Only Abilities display class
        [TypeConverterAttribute(typeof(AbilityConverter)),
        DescriptionAttribute("Bitfield describing abilities.")]
        public class ReadOnlyAbilities
        {
            bool _rc;
            [DescriptionAttribute("Whether the RC Receiver channel inputs are enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_RC { get { return _rc; } }
            bool _acc;
            [DescriptionAttribute("Whether the Accelerometer is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_ACC { get { return _acc; } }
            bool _gyro;
            [DescriptionAttribute("Whether the gyroscope is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_GYRO { get { return _gyro; } }
            bool _encoders;
            [DescriptionAttribute("Whether the wheel rotation encoders are present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_ENCODERS { get { return _encoders; } }
            bool _drift;
            [DescriptionAttribute("Whether the Drift Correction algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_DRIFT { get { return _drift; } }
            bool _roll;
            [DescriptionAttribute("Whether the Roll Stabilization algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_ROLL { get { return _roll; } }
            bool _balance;
            [DescriptionAttribute("Whether the Self Balance algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_BALANCE { get { return _balance; } }
            bool _poshold;
            [DescriptionAttribute("Whether the Position Hold algorithm is present and enabled."),
            DefaultValueAttribute(true)]
            public bool ABILITY_POSHOLD { get { return _poshold; } }

            public ReadOnlyAbilities()
            {
                _rc = _acc = _gyro = _encoders = _roll = _drift = _balance = _poshold = true;
            }

            public void Set(int mask)
            {
                _rc = ((mask & 1) == 1);
                _acc = ((mask & 2) == 2);
                _gyro = ((mask & 4) == 4);
                _encoders = ((mask & 8) == 8);
                _drift = ((mask & 16) == 16);
                _roll = ((mask & 32) == 32);
                _balance = ((mask & 64) == 64);
                _poshold = ((mask & 128) == 128);
            }

            public int Get()
            {
                return (_rc ? 1 : 0) + (_acc ? 2 : 0) + (_gyro ? 4 : 0) + (_encoders ? 8 : 0) + (_drift ? 16 : 0) + (_roll ? 32 : 0) + (_balance ? 64 : 0) + (_poshold ? 128 : 0);
            }

            public ReadOnlyAbilities(int mask)
            {
                Set(mask);
            }

            public override string ToString()
            {
                if (Get() == 0) return "NONE";
                if (Get() == 255) return "ALL";
                return (_rc ? "RC " : "") + (_acc ? "ACC " : "") + (_gyro ? "GYRO " : "") + (_encoders ? "ENCODERS " : "") + (_drift ? "DRIFT " : "") + (_roll ? "ROLL " : "") + (_balance ? "BALANCE " : "") + (_poshold ? "POSHOLD" : "");
            }

        }

        public class AbilityConverter : ExpandableObjectConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context,
                                          System.Type destinationType)
            {
                if (destinationType == typeof(Abilities)) return true;
                if (destinationType == typeof(ReadOnlyAbilities)) return true;
                return base.CanConvertTo(context, destinationType);
            }

            public override object ConvertTo(ITypeDescriptorContext context,
                                   CultureInfo culture,
                                   object value,
                                   System.Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is Abilities)
                {
                    Abilities c = (Abilities)value;
                    return c.ToString();
                }
                if (destinationType == typeof(System.String) && value is ReadOnlyAbilities)
                {
                    ReadOnlyAbilities c = (ReadOnlyAbilities)value;
                    return c.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (context.Instance is ReadOnlyAbilities) return false;
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = ((string)value).ToUpper();
                        int val = 0;
                        if (s.Contains("RC")) val += 1;
                        if (s.Contains("ACC")) val += 2;
                        if (s.Contains("GYRO")) val += 4;
                        if (s.Contains("ENCODER")) val += 8;
                        if (s.Contains("DRIFT")) val += 16;
                        if (s.Contains("ROLL")) val += 32;
                        if (s.Contains("BALANCE")) val += 64;
                        if (s.Contains("POSHOLD")) val += 128;
                        Abilities c = new Abilities(val);
                        return c;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type Abilities");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

        //Bitmask? not used..

        [TypeConverterAttribute(typeof(BitMaskConverter)),
        DescriptionAttribute("Expand to alter bitmask.")]
        public class BitMask : INotifyPropertyChanged
        {
            public Command BMcommand;
            public string Name;

            [DescriptionAttribute("Decimal value."),
            DefaultValueAttribute(255)]
            byte Value { get { return Get(); } set { Set(value); } }

            bool _b7;
            [DescriptionAttribute("Bit 7 MSB."),
            DefaultValueAttribute(true)]
            public bool B7
            {
                get { return _b7; }
                set { SetField(ref _b7, value, "B7", BMcommand); }
            }

            bool _b6;
            [DescriptionAttribute("Bit 6."),
            DefaultValueAttribute(true)]
            public bool B6
            {
                get { return _b6; }
                set { SetField(ref _b6, value, "B6", BMcommand); }
            }

            bool _b5;
            [DescriptionAttribute("Bit 5."),
            DefaultValueAttribute(true)]
            public bool B5
            {
                get { return _b5; }
                set { SetField(ref _b5, value, "B5", BMcommand); }
            }

            bool _b4;
            [DescriptionAttribute("Bit 4."),
            DefaultValueAttribute(true)]
            public bool B4
            {
                get { return _b4; }
                set { SetField(ref _b4, value, "B4", BMcommand); }
            }

            bool _b3;
            [DescriptionAttribute("Bit 3."),
            DefaultValueAttribute(true)]
            public bool B3
            {
                get { return _b3; }
                set { SetField(ref _b3, value, "B3", BMcommand); }
            }

            bool _b2;
            [DescriptionAttribute("Bit 2."),
            DefaultValueAttribute(true)]
            public bool B2
            {
                get { return _b2; }
                set { SetField(ref _b2, value, "B2", BMcommand); }
            }

            bool _b1;
            [DescriptionAttribute("Bit 1."),
            DefaultValueAttribute(true)]
            public bool B1
            {
                get { return _b1; }
                set { SetField(ref _b1, value, "B1", BMcommand); }
            }

            bool _b0;
            [DescriptionAttribute("Bit 0."),
            DefaultValueAttribute(true)]
            public bool B0
            {
                get { return _b0; }
                set { SetField(ref _b0, value, "B0", BMcommand); }
            }


            public BitMask(byte mask, Command command, string name)
            {
                B0 = B1 = B2 = B3 = B4 = B5 = B6 = B7 = true;
                this.BMcommand = command;
                this.Name = name;
                Set(mask);
            }

            public void Set(byte mask)
            {
                bool changed = false;
                bool bit;
                bit = ((mask & 1) == 1);
                if (_b0 != bit) { _b0 = bit; changed = true; }
                bit = ((mask & 2) == 2);
                if (_b1 != bit) { _b1 = bit; changed = true; }
                bit = ((mask & 4) == 4);
                if (_b2 != bit) { _b2 = bit; changed = true; }
                bit = ((mask & 8) == 8);
                if (_b3 != bit) { _b3 = bit; changed = true; }
                bit = ((mask & 16) == 16);
                if (_b4 != bit) { _b4 = bit; changed = true; }
                bit = ((mask & 32) == 32);
                if (_b5 != bit) { _b5 = bit; changed = true; }
                bit = ((mask & 64) == 64);
                if (_b6 != bit) { _b6 = bit; changed = true; }
                bit = ((mask & 128) == 128);
                if (_b7 != bit) { _b7 = bit; changed = true; }
                if (changed) OnPropertyChanged(Name, BMcommand);
            }

            public byte Get()
            {
                byte value = 0;
                if (B0) value += 1;
                if (B1) value += 2;
                if (B2) value += 4;
                if (B3) value += 8;
                if (B4) value += 16;
                if (B5) value += 32;
                if (B6) value += 64;
                if (B7) value += 128;
                return value;
            }

            public override string ToString()
            {
                return (B7 ? "1" : "0") + (B6 ? "1" : "0") + (B5 ? "1" : "0") + (B4 ? "1" : "0") + (B3 ? "1" : "0") + (B2 ? "1" : "0") + (B1 ? "1" : "0") + (B0 ? "1" : "0");
            }

            public event PropertyChangedEventHandler PropertyChanged;

            private void OnPropertyChanged(String info, Command command)
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs(info, command));
                }
            }

            protected bool SetField<T>(ref T field, T value, string propertyName, Command command)
            {
                if (EqualityComparer<T>.Default.Equals(field, value)) return false;
                field = value;
                OnPropertyChanged(propertyName, command);
                return true;
            }
        }

        
        public class BitMaskConverter : ExpandableObjectConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context,
                                          System.Type destinationType)
            {
                if (destinationType == typeof(BitMask)) return true;
                return base.CanConvertTo(context, destinationType);
            }

            public override object ConvertTo(ITypeDescriptorContext context,
                                   CultureInfo culture,
                                   object value,
                                   System.Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is BitMask)
                {
                    BitMask bm = (BitMask)value;
                    return bm.ToString();
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = (string)value;
                        byte val = 0;
                        for (int bit = 0; bit < s.Length; bit++)
                            if (s[bit] != '0') val += (byte)(2 ^ bit);
                        BitMask original = (BitMask)context.PropertyDescriptor.GetValue(context.Instance);
                        BitMask bm = new BitMask(val, original.BMcommand, original.Name);
                        return bm;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type BitMask");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

        //LimitPair display class
        [TypeConverterAttribute(typeof(LimitPairConverter)),
        DescriptionAttribute("Expand to alter limits.")]
        public class LimitPair :INotifyPropertyChanged
        {
            public robotSettings Settings;

            float _lower;
            [DescriptionAttribute("Lower limit of value."),
            DefaultValueAttribute(-255)]
            public float Lower
            {
                get { Settings.checkCurrent(lowerCommand); return _lower; }
                set { SetField(ref _lower, value, "Lower", lowerCommand); }
            }

            float _upper;
            [DescriptionAttribute("Upper limit of value."),
            DefaultValueAttribute(255)]
            public float Upper
            {
                get { Settings.checkCurrent(upperCommand); return _upper; }
                set { SetField(ref _upper, value, "Upper", upperCommand); }
            }

            //Multiple LimitPair objects are present in the Settings structure. Therefore
            // the class gets given the specific commands to interact with on construction.
            public Command lowerCommand, upperCommand;
            public LimitPair(robotSettings Settings, Command lowerCommand, Command upperCommand)
            {
                this.Settings = Settings;
                _lower = -255;
                _upper = 255;
                this.lowerCommand = lowerCommand;
                this.upperCommand = upperCommand;
            }

            //PropertyChanged stuff
            public event PropertyChangedEventHandler PropertyChanged;

            private void OnPropertyChanged(String info, Command command)
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs(info, command));
                }
            }

            protected bool SetField<T>(ref T field, T value, string propertyName, Command command)
            {
                if (EqualityComparer<T>.Default.Equals(field, value)) return false;
                field = value;
                OnPropertyChanged(propertyName, command);
                return true;
            }
        }

        //Get LimitPair to play nice with PropertyGrid
        public class LimitPairConverter : ExpandableObjectConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context,
                                          System.Type destinationType)
            {
                if (destinationType == typeof(LimitPair)) return true;
                return base.CanConvertTo(context, destinationType);
            }

            public override object ConvertTo(ITypeDescriptorContext context,
                                   CultureInfo culture,
                                   object value,
                                   System.Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is LimitPair)
                {
                    LimitPair lp = (LimitPair)value;
                    return lp.Lower.ToString("F2") + " to " + lp.Upper.ToString("F2");
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        string s = (string)value;
                        string[] values = s.Split('t');
                        values[1] = values[1].Substring(1);
                        //Need to extract Commands from original class
                        var original = (LimitPair)context.PropertyDescriptor.GetValue(context.Instance);
                        //Copy Commands over to new LimitPair
                        LimitPair lp = new LimitPair(original.Settings, original.lowerCommand, original.upperCommand);
                        //Set limits
                        float val;
                        float.TryParse(values[0].Trim(), out val);
                        lp.Lower = val;
                        float.TryParse(values[1].Trim(), out val);
                        lp.Upper = val;
                        return lp;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type LimitPair");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

        //PID display class
        [TypeConverterAttribute(typeof(PidConverter)),
        DescriptionAttribute("Expand to alter PID settings.")]
        public class PID:INotifyPropertyChanged
        {
            public robotSettings Settings;

            float _p;
            [DescriptionAttribute("PID controller P (Proportional) constant term."),
            DefaultValueAttribute(1),
            PropertyOrder(20)]
            public float P
            {
                get { Settings.checkCurrent(PCommand); return _p; }
                set { SetField(ref _p, value, "P", PCommand); }
            }

            float _i;
            [DescriptionAttribute("PID controller I (Integral) constant term."),
            DefaultValueAttribute(0),
            PropertyOrder(21)]
            public float I
            {
                get { Settings.checkCurrent(ICommand); return _i; }
                set { SetField(ref _i, value, "I", ICommand); }
            }

            float _d;
            [DescriptionAttribute("PID controller D (Derivative) constant term."),
            DefaultValueAttribute(0),
            PropertyOrder(22)]
            public float D
            {
                get { Settings.checkCurrent(DCommand); return _d; }
                set { SetField(ref _d, value, "D", DCommand); }
            }

            LimitPair _outputLimits;
            Command[] outputCommand;
            [DescriptionAttribute("Output limits. The output value is not allowed to go outside these limits."),
            DefaultValueAttribute(0),
            PropertyOrder(23)]
            public LimitPair OutputLimits
            {
                get { Settings.checkCurrent(gainCommand); return _outputLimits; }
                set
                {
                    SetField(ref _outputLimits, value, "OutputLimits", outputCommand);
                    value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
                }
            }

            LimitPair _gainLimits;
            Command[] gainCommand;
            [DescriptionAttribute("Gain limits. The integrated error (I term) is not allowed to go outside these limits."),
            DefaultValueAttribute(0),
            PropertyOrder(24)]
            public LimitPair GainLimits
            {
                get { Settings.checkCurrent(new Command[] { _gainLimits.lowerCommand, _gainLimits.upperCommand }); return _gainLimits; }
                set
                {
                    SetField(ref _gainLimits, value, "GainLimits", gainCommand);
                    value.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
                }
            }

            //Multiple PIDs in the Settings class. Each PID value needs to know it's specific 
            // related Command, different for each PID instance. These are passed in to the 
            // constructor here.
            public Command PCommand, ICommand, DCommand;
            public PID(robotSettings Settings, Command PCommand, Command ICommand, Command DCommand, Command GainLower, Command GainUpper, Command OutputLower, Command OutputUpper)
            {
                this.Settings = Settings;
                _p = 1;
                _i = 0;
                _d = 0;
                this.PCommand = PCommand;
                this.ICommand = ICommand;
                this.DCommand = DCommand;
                //Combined command for LimitPair class replacement
                gainCommand = new Command[] { GainLower, GainUpper };
                outputCommand = new Command[] { OutputLower, OutputUpper };
                //New LimitPairs - manually set up PropertyChanged
                _gainLimits = new LimitPair(Settings, GainLower, GainUpper);
                _gainLimits.PropertyChanged += new PropertyChangedEventHandler(childPropertyChanged);
                _outputLimits = new LimitPair(Settings, OutputLower, OutputUpper);
                _outputLimits.PropertyChanged +=new PropertyChangedEventHandler(childPropertyChanged);
            }

            //Handed down from LimitPairs
            void childPropertyChanged(object sender, PropertyChangedEventArgs e)
            {
                CommandChangedEventArgs c = (CommandChangedEventArgs)e;
                OnPropertyChanged(c.PropertyName, c.command);
            }

            //PropertyChanged stuff
            public event PropertyChangedEventHandler PropertyChanged;

            private void OnPropertyChanged(String info, Command command)
            {
                if (PropertyChanged != null)
                {
                    PropertyChanged(this, new CommandChangedEventArgs(info, command));
                }
            }

            protected bool SetField<T>(ref T field, T value, string propertyName, Command command)
            {
                if (EqualityComparer<T>.Default.Equals(field, value)) return false;
                field = value;
                OnPropertyChanged(propertyName, command);
                return true;
            }

            //Multi-command SetField
            protected bool SetField<T>(ref T field, T value, string propertyName, Command[] commands)
            {
                if (EqualityComparer<T>.Default.Equals(field, value)) return false;
                field = value;
                foreach (Command command in commands)
                    OnPropertyChanged(propertyName, command);
                return true;
            }
        }

        //Get PID to play nice with PropertyGrid
        public class PidConverter : ExpandableObjectConverter
        {
            public override bool CanConvertTo(ITypeDescriptorContext context,
                                          System.Type destinationType)
            {
                if (destinationType == typeof(PID)) return true;
                return base.CanConvertTo(context, destinationType);
            }

            public override object ConvertTo(ITypeDescriptorContext context,
                                   CultureInfo culture,
                                   object value,
                                   System.Type destinationType)
            {
                if (destinationType == typeof(System.String) && value is PID)
                {
                    PID pid = (PID)value;
                    return "P=" + pid.P.ToString("F2") + ", I=" + pid.I.ToString("F2") + ", D=" + pid.D.ToString("F2");
                }
                return base.ConvertTo(context, culture, value, destinationType);
            }
            public override bool CanConvertFrom(ITypeDescriptorContext context,
                                  System.Type sourceType)
            {
                if (sourceType == typeof(string)) return true;
                return base.CanConvertFrom(context, sourceType);
            }

            public override object ConvertFrom(ITypeDescriptorContext context,
                                  CultureInfo culture, object value)
            {
                if (value is string)
                {
                    try
                    {
                        //Chop up string
                        string s = (string)value;
                        string[] values = s.Split('=');
                        values[1] = values[1].Substring(0, values[1].IndexOf(','));
                        values[2] = values[2].Substring(0, values[2].IndexOf(','));
                        //Need to get original class
                        PID original = (PID)context.PropertyDescriptor.GetValue(context.Instance);
                        //Copy Commands over to new PID class
                        PID pid = new PID(original.Settings, original.PCommand, original.ICommand, original.DCommand, original.GainLimits.lowerCommand, original.GainLimits.upperCommand, original.OutputLimits.lowerCommand, original.OutputLimits.upperCommand);
                        //Fill in new LimitPairs from original class
                        pid.GainLimits.Lower = original.GainLimits.Lower;
                        pid.GainLimits.Upper = original.GainLimits.Upper;
                        pid.OutputLimits.Lower = original.OutputLimits.Lower;
                        pid.OutputLimits.Upper = original.OutputLimits.Upper;
                        //Set new values 
                        float val;
                        float.TryParse(values[1], out val);
                        pid.P = val;
                        float.TryParse(values[2], out val);
                        pid.I = val;
                        float.TryParse(values[3], out val);
                        pid.D = val;
                        return pid;
                    }
                    catch
                    {
                        throw new ArgumentException(
                            "Can not convert '" + (string)value +
                                               "' to type PID");
                    }
                }
                return base.ConvertFrom(context, culture, value);
            }
        }

    }
    public class UnknownCommandException : Exception
    {
        public UnknownCommandException() { }

        public UnknownCommandException(string message)
            : base(message) { }
        public UnknownCommandException(string message, Exception inner)
            : base(message, inner) { }
    }
}
